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EXECUTIVE  SUMMARY 


This  report  considers  the  vehicle  navigation  problem  for  an  autonomous  underwater  vehicle  with  six 
degrees  of  freedom.  We  approach  this  problem  using  an  error  state  formulation  of  the  Kalman  filter. 
Integration  of  the  vehicle’s  high-rate  inertial  measurement  unit’s  accelerometers  and  gyros  allow 
time  propagation  while  other  sensors  provide  measurement  corrections.  The  low-rate  aiding  sensors 
include  a  Doppler  velocity  log,  an  acoustic  long  baseline  system  that  provides  round-trip  travel  times 
from  known  locations,  a  pressure  sensor  for  aiding  depth,  and  an  attitude  sensor.  Measurements 
correct  the  filter  independently  as  they  arrive,  and  as  such,  the  filter  is  not  dependent  on  the  arrival 
of  any  particular  measurement.  The  navigation  system  can  estimate  critical  bias  parameters  that 
improve  performance.  The  result  is  a  robust  navigation  system.  Simulation  and  experimental 
results  are  provided. 


1.  INTRODUCTION 

With  the  emergence  of  inspection-class  autonomous  underwater  vehicles,  navigation  and  naviga¬ 
tional  accuracy  are  becoming  increasingly  important.  Without  an  operator  in  the  loop,  the  vehicle 
itself  must  use  sensors  to  determine  its  location,  orientation,  and  motion.  Many  of  these  unique 
sensors  rely  on  acoustic  measurements  that  present  interesting  challenges.  The  problem  is  how 
to  effectively  use  all  available  sensor  inputs  to  provide  a  continuous  and  robust  estimate  of  the 
vehicle’s  navigational  state. 

One  approach  is  to  treat  each  sensor  independently,  each  measuring  a  specific  state.  A  position 
sensor  measures  position,  a  velocity  sensor  measures  velocity,  and  so  forth.  This  solution,  however, 
is  clearly  not  robust  and  does  not  take  advantage  of  the  kinematic  relationships  between  states 
and  measurements.  Consider  the  situation  where  sensor  performance  degrades  and  measurement 
updates  become  sporadic.  If  position  fixes  are  not  regularly  available,  how  should  the  position  state 
evolve?  One  could  dead-reckon  with  the  velocity  and  heading,  then  blend  this  estimate  with  each 
position  fix,  but  is  this  ad-hoc  method  optimal? 

Here,  we  present  an  approach  using  an  error  state  formulation  of  the  Kalman  filter  to  provide 
an  optimal  and  robust  solution  to  the  vehicle  navigation  problem.  This  report  concentrates  on  the 
application  of  the  Kalman  filter  and  development  of  the  model  and  filter  algorithms.  It  does  not 
attempt  to  justify  the  Kalman  filter  [1,  2,  3]  or  make  comparisons  to  other  algorithms. 

This  report  is  an  extension  of  previous  work  [4].  Here,  we  expand  upon  our  approach  and 
analysis,  and  reformulate  algorithms  to  provide  a  more  theoretically  concrete  implementation.  The 
following  section  describes  the  system  of  interest.  Section  2  develops  the  continuous-time  model 
for  this  system,  while  Section  3  formulates  the  corresponding  navigation  equations.  In  Section  4, 
we  choose  several  interesting  scenarios  to  analyze  critical  aspects  of  our  approach.  The  final  two 
sections  include  experimental  results  and  a  discussion  of  potential  future  work. 

1.1  SYSTEM  DESCRIPTION 

The  particular  system  of  interest  is  an  underwater  vehicle  with  six  degrees  of  freedom.  The  ve¬ 
hicle  propels  itself  via  multiple  thrusters,  allowing  for  a  variety  of  dynamic  maneuvers.  From  a 
navigation  standpoint,  we  assume  that  it  can  rotate  and  translate  in  any  direction,  by  actuation  or 
environmental  disturbances.  The  vehicle’s  nominal  operating  speed  is  approximately  1  knot,  and 
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it  has  an  operating  area  on  the  order  of  100  x  100  with  a  maximum  depth  of  less  than  50  m. 

A  unique  suite  of  on-board  sensors  provide  information  related  to  the  vehicle’s  motion.  The 
primary  sensor  is  an  inertial  measurement  unit  that  measures  accelerations  and  angular  rates  in 
three  dimensions.  This  sensor  is  reliable,  but  due  to  noise  and  unknown  biases,  it  alone  cannot 
provide  sufficient  navigational  accuracy.  Other  sensors  provide  additional  feedback.  A  Doppler 
velocity  log  provides  velocities  along  four  beam  directions  via  acoustic  Doppler  measurements.  An 
acoustic  long  baseline  (LBL)  system  measures  round-trip  travel  times  between  a  transceiver  on  the 
vehicle  and  four  transponder  baseline  stations  at  known  locations.  An  attitude  and  pressure  sensor 
complete  the  navigation  suite.  The  attitude  sensor  provides  orientation  measurements,  while  the 
pressure  sensor  provides  a  sense  of  vehicle  depth.  We  assume  the  platform-frame  sensor  locations 
are  known  exactly  and  that  all  measurements,  except  for  the  LBL,  have  negligible  measurement 
latencies.  Delay  is  inherent  in  the  LBL  system,  and  methods  to  address  that  delay  to  enhance 
navigation  accuracy  are  a  major  contribution  of  this  article.  Figure  1  illustrates  the  general  sensor 
configuration. 


Figure  1 :  Vehicle  sensor  configuration. 


1 .2  NOTATION 

We  use  the  following  notation  [5].  For  vehicle  position  and  attitude,  define  tangent  plane  position 
rji  =  [x,y,z]^  and  attitude  r]2  =  Vehicle  position  is  defined  in  local  tangent  plane 

coordinates,  where  x  aligns  with  north,  y  aligns  with  east,  and  z  is  down.  Euler  attitude  angles 
are  roll  {(p),  pitch  (6),  and  yaw  (V^).  In  the  vehicle  or  platform  frame,  define  platform  velocity 
ui  =  [u,  angular  rates  1^2  =  \p,q,r]^ ,  and  linear  acceleration  =  [o^,  a^,, 


2.  MODEL  DERIVATION 

Given  limited  knowledge  of  system  dynamics  and  environmental  uncertainties,  a  kinematic  model 
is  often  preferable  to  a  complex  dynamic  model.  More  importantly,  kinematics  are  exact,  with 
no  uncertain  parameters.  Dynamics  are  not.  This  kinematic  model  relates  platform  accelerations, 
velocities,  and  angular  rates  to  changes  in  tangent  plane  position  and  attitude.  It  does  not  account 
for  vehicle  or  fluid  dynamics,  or  other  environmental  forces,  thus  allowing  the  navigation  algorithms 
to  be  platform  independent. 
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The  inertial  frame  of  reference  i  is  at  the  Earth  center  and  is  non-accelerating,  non-rotating, 
and  has  no  gravitational  field.  The  Earth-center/Earth- fixed  (ECEE)  frame  e  is  coincident  with 
and  rotates  about  the  inertial  frame  at  a  known  constant  rate,  cuj/g.  On  the  surface  of  the  Earth 
lies  tangent  frame  t,  which  is  fixed  within  the  ECEE  frame.  The  inertial  acceleration  expressed  in 
the  tangent  frame  is 


=  Rl 


+ 


where  is  the  skew  symmetric  matrix  form  of  rotation  rate  cross  product  of  frame  t 

with  respect  to  frame  i,  represented  in  frame  t.  Vector  r  describes  the  vehicle  position  relative  to 
the  inertial  origin.  Solving  for  f*  yields 


r*  = 

=  Rjif  +  G*)  -  -  ti\i/ 

=  f  +  (G*  - 


where  vectors  /  and  G  represent  the  specific  force  and  position  dependent  gravitational  acceleration. 
Vector  is  the  local  gravity  vector.  Note  that  =  Oj/g  +  ^e/t)  ^e/t  =  0- 

We  use  an  Euler  attitude  representation  to  describe  vehicle  orientation,  where  an  Euler  3-2-1 
rotation  sequence  [6]  defines  the  relationship  between  tangent  and  platform  frames.  It  is  important 
to  understand  that  each  Euler  angle  describes  a  rotation  about  an  axis  in  separate  frames.  We 
exploit  this  relationship  between  intermediate  frames  in  the  formulation  of  attitude  rates  and  in  the 
measurement  correction  of  attitude  angles.  The  combined  rotation  sequence  from  tangent  frame  t 
to  platform  frame  p  is 


cOcip 

=  C'tpsOscj)  —  ccpsip 
ccpcipsO  +  scps^ 


cOsip 

-sO 

ccpcip  +  sOscpsip 

c6s(j) 

—cipscp  -|-  c4>s6s^ 

c6c(j) 

with  inverse  Rp  =  (i?f)  ^  =  {R^)~^ .  The  velocity  vectors  in  platform  and  tangent  frames  are  related 
by  u*  =  RpVP  and 

yP  =  RPv\  (2) 

The  time  derivative  of  the  second  equation  is 

yP  =  RPy^  +  RPtV^ 

=  RP{f  +  g^-  +  {-nlfRP)R^pyP 

=  aP  -  2nP,  yP  -  (QP,  -  QP,  )vP 

=  a-  -  Rtn\i^Ry  - 

where  we  have  expressed  in  the  inertial  frame  using  equation  (1).  Angular  rate  is  computed 
from  the  gyro  measurement  by  removing  biases.  Platform  acceleration  aP  is  computed  directly  from 
the  accelerometer  specific  force  measurement  by  compensating  for  gravity  and  removing  biases. 

Recall  that  each  Euler  angle  defines  a  rotation  about  an  axis  in  separate  frames.  To  relate  the 
Euler  attitude  rates  {(p,6,^)  to  angular  rates  in  the  platform  frame,  relate  rotation  axes  (?i, 52,^3) 
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in  their  respective  intermediate  frames  to  platform  frame  coordinates  {ip,jp,kp).  For  example, 
=  R2  i^)Ri  W[h, h,hV ■  Let 


^t/p  =  +  ^32  +  'fph 

=  (pip  +  9{c(j)jp  —  s(pkp)  —  ip{s6ip  —  c9s(pjp  —  c9c4>kp) 

=  {(p  —  ips9)ip  +  {9c(j)  +  ipc9s(j))jp  +  {'ipc9c4>  —  9s(j))kp 

be  the  vector  expression  of  angular  rates  in  platform  frame  p  relative  to  the  tangent  frame  t, 
represented  in  frame  p.  In  matrix  form  and  using  notation  from  Section  1.2,  the  Euler  and  platform 
angular  rates  are  related  by  122  =  and 


7)2  =  O  ^2, 


where  =  uj^/p  -  wf/, 


m  =  [^,9,'ip]'^, 

and 

■  1 

0 

-s9 

n  = 

0 

C(j) 

c9s(p 

0 

—  S(j) 

c9c(j) 

(4) 


Note  that  the  inverse  relationship  approaches  a  singularity  as  0  ^  It  is  assumed  the  vehicle 
will  not  operate  near  this  singularity.  If  operation  near  is  desired,  then  an  alternative  attitude 
representation,  such  as  quaternions,  would  remove  this  singularity. 

After  equations  (2)-(4)  are  summarized  using  the  notation  from  Section  1.2,  the  continuous-time 
kinematic  model  is 

f]i  =  RpUi 

7)2  =  n~^U2  (5) 

The  next  section  utilizes  this  model,  along  with  bias  and  measurement  models,  to  propagate  the 
system  through  time  and  formulate  measurement  predictions. 


3.  NAVIGATION 

Given  system  output  y,  we  wish  to  reconstruct  the  entire  internal  state  x.  From  linear  systems 
theory,  we  know  that  if  the  discrete  system 

Xk+i  =  ^Xk  ToJk 
Vk  =  Hxk 

is  observable,  then  we  can  design  the  observer 

Xfc+i  =  -h  L{yk  -  ilk) 

Vk  =  Hxk 

such  that  the  eigenvalues  of  (<I>  —  LH)  lie  within  the  unit  circle.  This  condition  is  necessary  for 
asymptotic  stability  of  the  error  state  system,  defined  by 

5xk+i  =  (4>  -  LH)5xk 
5yk  =  H5xk. 
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Asymptotic  stability  of  the  error  state  system  causes  6xk  to  converge  exponentially  to  zero,  thereby 
estimating  Xk-  We  use  notation  5xk,  as  opposed  to  Xk,  to  indicate  the  expected  value  of  the  error 
rather  than  the  true  error.  For  time-varying  systems,  Lk  must  stabilize  —  LkHk)  for  all  time 
[7,  8], 

Here,  we  employ  the  Kalman  filter  to  design  Lk  in  an  optimal  manner,  given  noisy  measurements 
y.  The  Kalman  filter  is  optimal  in  the  mean-squared  sense  [1,  2,  3].  In  the  following  sections,  we 
formulate  system  error  state  equations  and  apply  the  Kalman  filter.  Our  formulation  revolves 
around  the  system’s  inertial  measurement  unit  (IMU),  and  as  such,  we  describe  this  sensor  first  in 
Section  3.1.  Section  3.2  defines  the  augmented  system  equations.  These  equations  model  the  true 
system,  while  Section  3.3  defines  the  mechanization  equations  that  provide  the  navigation  state 
vector  X.  The  difference  between  the  actual  and  mechanized  systems  is  the  error  state  system.  A 
model  for  the  error  state  system  is  described  in  Section  3.4.  We  use  the  error  state  system  to  design 
the  Kalman  filter.  Section  3.5  applies  the  Kalman  filter  time  propagation  equations 


=  ^k^x^  (6) 

Pk+i  =  Wk^i  +  Qd,  (7) 

where  equation  (6)  propagates  the  error  state  5x  and  equation  (7)  propagates  the  error  state 
covariance  Pk-  Section  3.6  applies  the  Kalman  filter  measurement  correction  equations 

Kk  =  P^HjiHkP^Hl  +Rk)-^  (8) 

^Vk  =  Vk-  h{xk)  (9) 

dx^  =  5x'^  +  Kk{5yk  -  HkSx'^)  (10) 

P+  =  {I-KkHk)Pk,  (11) 


to  the  aiding  sensors.  Equation  (8)  computes  the  optimal  Kalman  gain,  equation  (9)  computes  the 
measurement  residual,  equation  (10)  corrects  the  error  state  estimate,  and  equation  (11)  updates  the 
error  state  covariance  matrix.  Circular  (i.e.,  angular)  measurements  should  correct  the  innovation 
{5yk  —  H5x^)  to  lie  on  interval  [— vr,  vr).  Superscript  “  indicates  the  a  priori  state  or  covariance 
at  time  k  immediately  prior  to  the  innovation.  Superscript  indicates  the  a  posteriori  state  or 
covariance  at  time  k  immediately  after  the  innovation.  Kalman  gain  Kk  is  the  optimal  observer 
gain  Lk  at  instant  k.  After  a  measurement  correction,  the  filter  should  initialize  the  a  priori  state 
and  covariance  to  the  a  posteriori  state  and  covariance  for  the  next  filter  iteration  (i.e.,  x^^i  =  x^ 
and  =  4+). 

Recognizing  the  difference  between  the  error  state  5x  and  navigation  state  x  is  important.  The 
navigation  mechanization  computes  x  by  integration  of  the  IMU  data  between  time  instants  tk 
and  tk+i,  at  which  aiding  measurements  are  available.  At  such  time  instants,  x  is  used  to  predict 
the  measurement.  The  filter  uses  the  residual  between  the  actual  and  predicted  measurements  to 
estimate  5x'^ .  When  6x'^  is  available,  the  navigation  state  x  correction  is 

x'^  =  x~  +  5x'^ .  (12) 

After  each  navigation  state  correction,  error  states  5x~  and  6x'^  are  zero  since  the  navigation  state 
estimate  now  incorporates  this  information.  Clearly,  if  we  correct  x  immediately  after  each  6x 
correction,  5x~  is  always  zero,  and  it  is  not  necessary  to  propagate  5x~  as  in  equation  (6). 
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3.1  INERTIAL  MEASUREMENT  UNIT 


The  inertial  measurement  unit  (IMU)  is  the  primary  high-rate  sensor.  It  measures  linear  acceler¬ 
ations  via  accelerometers  and  angular  rates  via  fiber-optic  gyros.  The  IMU  effective  measurement 
point  defines  the  origin  of  the  platform  frame.  The  IMU  also  provides  an  accurate  time  measure¬ 
ment  since  the  last  update.  This  delta  time  measurement  is  useful  for  precise  integration,  later 
described  in  Section  3.5.  We  expect  this  sensor  to  provide  continuous  updates  without  interruption. 

The  accelerometer  specific  force  and  gyro  measurement  outputs,  ya  and  yg,  respectively,  are 
modeled  by 


ya  =  aF  -  Rt9^  +  ba  + 


rin 


(13a) 

(13b) 


yg  =  +  ^9  + 

where  {ba,na)  and  {bg,ng)  are  bias  and  noise  vectors.  Noise  vectors  Ua  and  Ub  are  distributed 
according  to  N{0,a‘^I)  and  N{0,a‘^I),  respectively,  and  are  assumed  to  be  white  noise  processes 
[1].  The  acceleration  and  angular  rate  estimates  are  computed  as 

(14a) 


=ya  +  Rt9^  -  ba 


U)- 


p  — 


ijp 


=  yg-  hg. 


Bias  vectors  ba  and  b„  are  modeled  as  random  constants  plus  random  walks,  where 


ba  — 
bg  =  ^g 


fea  =  0 

bg  =  0. 


(14b) 

(15a) 
(15b) 

The  driving  noise  vectors  cua  and  tOg  are  distributed  according  to  N{0,  cr'^a^)  and  N{0,  cr'^gl),  respec¬ 
tively. 

3.2  AUGMENTED  SYSTEM  EQUATIONS 

Augmenting  our  continuous-time  model,  in  equation  (5),  with  the  models  for  the  unknown  param¬ 
eters  in  equations  (15a)  and  (15b),  yields  the  system  state  vector 

x=[9i  92  bj  b]  c  ]  ^ . 

The  additional  parameter  c  represents  our  estimate  for  the  speed  of  sound  in  seawater.  We  model 
c  as  a  random  constant  plus  random  walk  with  driving  noise  tOc  ~  -^(OjO'c);  where  ac  =  0.1  m/s. 
This  estimate  is  necessary  to  calculate  round-trip  distances  from  travel  times  for  the  long  baseline 
system,  described  in  Section  3.6.3.  The  process  noise  input  vector  is 

T  T  T  T  n  T 


U 


n„ 


nj  wj  ujJ  LVc 


where  all  quantities  are  mutually  uncorrelated,  Gaussian,  white  noise  vectors.  The  augmented  true 
system  equations  are  therefore 

f]i  =  R^ui 

f]2  =  ^~^{yg  -bg-Ug-  R^i/e) 


i'l  =  (ya  +  Rt9^  -  ba-  na)  -  RlVtlj^RpUi  -  {yg  -  bg-  Ug)  x  ui 
ba  —  ^a 

bg  =  LOg 


(16) 


C  —  CUc? 
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where  we  have  substituted  for  and  1^2  with  equations  (14a)  and  (14b),  respectively,  in  preparation 
for  the  development  of  the  error  state  equations  in  Section  3.4. 


3.3  MECHANIZATION  EQUATIONS 

The  mechanization  equations  represent  the  expected  value  of  the  true  system.  Our  estimate  of  the 
true  system,  equation  (16),  is 

fji  = 

m  =  ^~Hy9  -bg-  RWi/^) 

=  iVa  +  Rt9^  -  ba)  -  -  {yg  -  bg)  x  z>i 

.  (17) 

&a  =  0 
bg  =  0 

c  =  0, 

where  we  assume  the  gravitation  vector  and  rotation  rate  are  deterministic. 

3.4  ERROR  STATE  EQUATIONS 

The  error  state  equations  represent  the  expected  value  of  the  error  between  the  true  system  and 
its  estimate,  6x  =  x  —  x.  To  compute  the  transformation  error  between  two  rotation  matrices,  we 
define  a  small-angle  transformation  as  R^  =  (I  —  [5px])Rp,  where  (/  —  [5/Ox])  represents  a  small- 
angle  transformation  from  the  true  tangent  frame  to  the  computed  tangent  frame  [9] .  The  quantity 
6p  represents  the  small-angle  error  between  true  and  computed  frames.  The  error  state  vector  is 
then 

5x  =  [  5riJ  6p~^  6iyJ  6bJ  6b^  (5c  j"*",  (18) 

where  5p  replaces  6r]2-  The  following  relations  are  useful  in  the  subsequent  analysis; 

Rp  =  {I  -  [Spx])Rp  (19a) 

Rl  =  {I +[5px])Rl  (19b) 

=  i?f(/  +  [(5px])  (19c) 

RP  =  RUl-[6px]).  (19d) 

Using  the  small-angle  relationships,  we  compute  the  error  state  equations  for  each  state.  The 
tangent  position  error  is 

Sm  =  m-m 

=  RpSui  +  [5px]Rp5i'i  +  [5px]Rpi'i  (20) 

ss  Rp6ui  —  [RpUix]Sp, 

where  we  have  manipulated  the  terms  such  that  the  error  state  coefficients  can  be  represented  in 
matrix  form.  To  determine  the  error  state  model  for  6p  [10],  differentiate  the  rotation  matrix  error 
5Rj,  =  [6px]Rl, 

5R\,  =  [5px]Rl+[5px]Rl 

=  [5px]Rl  +  [5px]{Rln^^^^), 
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and  solve  for  [5px] 


[,5/lx]  =  (i?*  -  Rl)Rl  -  [5px]RlaP^^Rl 

=  -  Rl,%^m  -  [5/9x])  -  [6px]Ql/^ 

~  ^t/p  ~^t/p  +  ^t/p[^P^]  ~  [^P>^]^t/p- 
Equivalently,  this  expression  written  in  vector  form  is 

^P  =  ‘^t/p  ~  ‘^t/p  +  ‘^t/p  ^  ^P 

=  K^^Rp  -  K^Rp  -  ^^Rt  +  i^l^Rp  -  ^Rt)  X  ^P 

=  K^^\ip  +  -  bu:\n  -  (R\h  X  bp 

~  ^p(--56g  -  ug)  -  bu\n  -  (R\n  X  bp, 

where  bLo\n  =  {— 0,  cos  d^/ dpi}bpi  and  ([)  is  the  vehicle  latitude.  The  platform  velocity 
error  is 

,5hi  =  Pi  —  Pi 


^r{b‘x]  •  RlPi)i}bp 


-{[{yp-bg)x]+R^,nl^^Rl\ 


-  bba  -  n„ 


-  [f>ix],56g  -  [j>ix]ng  +  R^bg^  +  Rl[RpPix]buj\i^. 

Remaining  error  expressions  for  bba,  bbg,  and  be  are  trivial  and,  as  such,  their  derivation  is  not 
shown.  The  resulting  continuous-time  error  state  system  is 


bx  =  Fbx  +  Gu, 


where 


0  —[RpPix]  Rp 


[RlPiX 

ije 

F32 


0  -Rp 


F33  —I  — [z^ix]  0 

0  0  0  0 

0  0  0  0 

0  0  0  0 


F21  =  duj\/Jdgi 

-^31  =  Rl\d9^/dgi  +  [RlPix]dJiiJdgi'^ 
F32  =  Rf{b‘x]  -  (4,  •  R^i 

F33  =  -[{Vg-W  -  R^t^ReK 


0  0  0  0 

-R*  0  0  0 

—  [z>ix]  0  0  0 

0  7  0  0 

0  0  7  0 

0  0  0  7 


Due  to  the  relatively  small  operating  area,  we  assume  F21  and  F31  are  approximately  zero.  Note 
that  the  error  state  vector  contains  6p,  while  the  navigation  state  contains  r]2-  The  measurement 
correction  routines  in  Section  3.6  will  account  for  the  use  of  5p. 

3.5  TIME  PROPAGATION 

The  time  propagation  routine  propagates  the  navigation  state,  error  state,  and  error  state  covariance 
through  time.  For  each  measurement  update  from  the  IMU,  the  time  propagation  routine  computes 
the  continuous-time  system,  the  discrete-time  system,  and  then  propagates  the  system  state  and 
covariance.  The  continuous-time  system  is  F  and  G  with  process  noise  distribution  matrix  Q, 
where  matrices  F  and  G  are  computed  according  to  equation  (23)  and  Q  =  E{uvJ).  We  assume 
Q  =  diag(iT^/,  cj^/,  (Tg).  To  compute  the  equivalent  discrete-time  system,  we  compute 

the  matrix  exponential 

„  (\  -F  GQG'^  1  .A 

using  algorithms  from  reference  [11].  Quantity  At  is  the  integration  period  from  the  IMU.  The 
result  is 

r=\-^  ' 

[0  J  ’ 

where  matrices  and  Qd  represent  the  discrete-time  system  [12].  Matrix  <h(A:  -|-  1,  A:)  propagates 
the  error  state  5x'^  and  covariance  Pj^ ,  where  P7  =  time  k  to  k  +  1  according  to 

equations  (6)  and  (7),  respectively.  Matrix  Qd  represents  the  discrete-time  process  noise  distribution 
matrix  at  time  k,  and  D  is  a  nonzero  dummy  matrix.  Given  T,  matrices  and  Qd  are  trivially 
solved  from  its  sub- matrices, 

^{k  +  l,k)  =  T[n -|- 1  :  2n,  n -|- 1  ;  2n]'''  (24) 

Qd{k)  =  ^{k  +  l,k)T[l  :  n,n  +  I  :  2n\,  (25) 

where  T[ri  :r2,ci  :  C2]  represents  rows  ri  through  r2  and  columns  ci  through  C2  of  the  2n  x  2n 
matrix  T. 

We  propagate  the  navigation  state  estimate  x~  using  a  predictor-corrector  integration  algorithm 

[13], 

=  +f{Xf^,yk)At 

4+1  =  Xk  +  /(4+1,  yfc)  At  (26) 

4+1  =  2K+i+^fc+i)> 

where  yk  =  [ya{k),yg{k)]^ ,  then  correct  the  resulting  attitude  angles  to  lie  on  interval  [— 7r,7r). 
Function  f{x,y)  is  the  continuous-time  mechanization  described  in  equation  (17). 

3.6  MEASUREMENT  CORRECTIONS 

In  this  section,  we  use  the  terms  sensor  and  measurement  to  refer  to  all  sensors  other  than  the  IMU. 
These  aiding  sensors  are  discussed  in  Sections  3.6.1  through  3.6.4.  Each  sensor  runs  independent  of 
the  next,  with  its  own  update  rate  and  performance  characteristics.  Thus,  measurement  corrections 
are  asynchronous.  As  a  measurement  arrives,  it  is  evaluated  and  then  incorporated  into  the  error 
state  estimate.  If  a  measurement  does  not  arrive,  no  calculations  are  necessary.  The  algorithm  does 
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not  wait  for  or  expect  measurements  to  arrive  in  an  ordered  fashion;  the  error  state  and  navigation 
state  will  propagate  according  to  equations  (6)  and  (26),  respectively,  via  the  IMU  data,  with  or 
without  measurement  corrections. 

Measurements  are  evaluated  with  several  sanity  checks.  One  such  check  verifies  that  the  mea¬ 
surement  lies  within  three  standard  deviations  of  its  estimate,  {5yk  —  <  9{HkP^ HJ  +  Rk), 

where  5yk  is  the  measurement  residual  and  matrix  {HkP^ +  Rk)  represents  the  measurement 
covariance.  Additional  logic  is  necessary  to  help  ensure  that  this  algorithm  does  not  disregard  valid 
measurements,  especially  upon  initialization. 

Valid  measurements  correct  the  error  state  estimate  according  to  equation  (10).  The  following 
sections  describe  the  low-rate  aiding  sensors  and  their  respective  measurement  correction.  Mea¬ 
surement  corrections  require  a  measurement  residual  6y,  sensor  output  matrix  H,  and  measurement 
noise  matrix  R  to  evaluate  equations  (8)  and  (10).  Note  that  the  error  state  vector  contains  6p, 
while  the  navigation  state  vector  contains  7)2-  We  cannot  simply  correct  the  attitude  states  of  x, 
as  described  in  equation  (12).  Instead,  we  use  7)2  to  correct  transformation  matrix  via  (19d), 
where 

/?r(7?2+)=i?r(r)2-)(/-[5px]),  (27) 

and  compute  1)2  from  the  resulting  transformation  matrix, 


where  R^[i,j\  represents  the  {i,j)  element  of  matrix  R^^fj^)  [12].  Function  arctan2(y,x)  is  the 
four-quadrant  arc  tangent  function.  To  ensure  numerical  stability,  it  is  necessary  to  normalize  i?f 
prior  to  evaluating  equations  (28a)-(28c). 

3.6.1  Attitude  Update 

The  attitude  sensor  combines  four  tilt  and  three  magnetometer  measurements  to  produce  roll,  pitch, 
and  yaw  information.  We  assume  the  vehicle  dynamics  are  slow,  and  as  such,  the  coupling  between 
the  inclinometers  and  platform  acceleration  is  negligible.  We  also  assume  the  magnetometer  is 
calibrated  to  compensate  for  hard  iron  characteristics  of  the  operating  region.  Momentary  magnetic 
spikes  can  easily  be  ignored.  The  sensor  model  is 

ye  =  ri2  +  ne, 

where  sensor  noise  Tie  is  distributed  according  to  iV(0,  Ug/).  We  can  predict  the  measurement  as 

Ve  =  m, 

and  compute  the  measurement  residual  as 

Sye  =  ye-ye  (29) 
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when  a  measurement  arrives.  To  formulate  the  sensor  output  matrix  H  and  measurement  noise 
matrix  ii,  a  theoretical  expression  for  the  measurement  residual  must  be  determined  in  terms  of 
the  error  state.  In  this  case,  an  expression  for  5ri2  in  terms  of  5p  must  be  found.  This  formulation 
is  similar  to  the  relationship  between  the  Euler  attitude  rates  and  platform  angular  rates.  Quantity 
5rj2  describes  attitude  error  relative  to  the  intermediate  rotation  axes  ^3),  while  5p  describes 

attitude  error  in  tangent  frame  By  definition, 

6p  =  dcpii  +  66  j2  +  d'lpks 

=  dtpicOcipit  +  cOsipjt  —  s6kt)  —  69{s'il)it  —  cipjt)  +  Sipkt 
=  {6(j)c6c'ijj  —  59s'ij:)it  +  {6(j)c9s'ip  +  69c'ip)jt  +  {6'ip  —  d(l)s9)kt 
=  ^6r]2, 


where 


E  = 


c9c'il>  —s'lp  0 
c9sil!  c'lp  0 
-sO  0  1 


The  theoretical  measurement  error  expression  is  therefore 


SVe  =  {m  +  ne}  -  {m} 

=  +  Ue 

=  H6x  +  Ue, 


where  the  sensor  output  matrix  is 


H  =[0  S-i  0  0  0  0  ]  .  (30) 

Note  that  approaches  a  singularity  as  9  ^  ±^.  The  measurement  noise  matrix  is 

R  =  E(^6y6y'^) 

=  E 
=  CTp, 

which  is  positive  definite  for  all  time.  Quantities  E[6x  and  R  represent  the  mean  and  covariance, 
respectively,  of  the  distribution  for  6y.  When  an  attitude  measurement  arrives,  the  Kalman  filter 
evaluates  equations  (29)-(31)  and  then  uses  the  results  in  (8)-(12),  and  (28a)-(28c)  to  correct  the 
state  estimate.  Note  that  the  Kalman  filter  assumes  5p  is  small;  therefore,  the  attitude  sensor 
should  initialize  572  with  the  latest  measurement  when  the  filter  initializes.  In  Section  4.1,  we  show 
that  yaw  is  observable  from  the  long  baseline  system  when  the  vehicle  has  a  nonzero  velocity,  thus 
the  magnetic  compass  can  be  disabled  if  necessary. 

3.6.2  Doppler  Velocity  Log  Update 

The  Doppler  velocity  log  (DVL)  measures  velocity  via  the  Doppler  effect  by  first  emitting  encoded 
acoustic  pulses  from  each  of  its  four  transducer  heads.  These  pulses  reflect  off  surfaces,  such 
as  the  seafloor,  and  return  back  to  each  transducer.  The  instrument  measures  the  change  in 
frequency  between  the  pulses  emitted  and  those  received,  which  relates  to  velocities  along  each 
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beam  direction  relative  to  the  reflecting  object.  In  certain  situations,  one  or  more  beams  may 
not  return  valid  information.  When  fewer  than  three  beams  return  valid  information,  we  cannot 
compute  geometric  transformation  [14]  to  relate  beam  velocities  to  instrument  frame  velocities.  It 
is  possible  to  alleviate  this  restriction.  Here,  we  treat  each  beam  velocity  as  a  separate  innovation. 
This  approach  does  not  require  bottom  lock  and  incorporates  more  information  into  the  filter  than 
an  instrument  frame  correction  (i.e.,  four  beam  corrections  vs.  three  orthogonal  corrections). 

Let  b  =  {6i,  62,  63,  64}  be  unit  vectors  along  each  beam  direction.  The  ith  Doppler  measurement 
is 

Uv  =  +  1^2  X  h)  •  bi  +  Uy, 

which  represents  the  instrument  frame  velocity  along  beam  direction  bi.  Vector  ii  is  the  transducer 
head  offset  from  the  origin  of  the  platform  frame,  and  Uy  is  sensor  noise  with  normal  distribution 
V(0,  cj^).  The  measurement  estimate  is  therefore 

Vv  =  {h  +  X  ii)  ■  bi, 

where  ii  and  bi  are  known  exactly,  with  residual 


Syv  =  yv-yv 

=  {{’^i  +  X  ii)  ■  bi  +  Uy}  -  {(z>i  +  z/2  X  ii)  ■  bi} 

=  bj  5  i2i  +  bj  [iix]6bg  +  bj  [iix]5ng  +  Uy. 


The  sensor  output  matrix  is 


H  = 


0  0  67  0  67  [^ix]  0 


and  the  measurement  noise  matrix  is 

R  =  E  [iix]ng  +  n„|  |67 [iix]ng  +  Uy'^ 

=  bJ  [iix]agl[iix]^bi  +  al, 


(32) 


(33) 


(34) 


which  is  positive  definite  for  all  time.  Note  the  significance  of  the  sensor  placement  in  relation  to 
the  gyro  bias  and  variance.  Offset  ii  provides  observability  to  the  gyro  bias  in  equation  (33),  while 
if  magnifies  the  effect  of  the  gyro  variance  in  equation  (34).  We  ignore  any  correlation  between 
the  process  noise  Ug  and  measurement  noise  vectors.  When  a  DVL  beam  measurement  arrives,  the 
Kalman  filter  evaluates  equations  (32)~(34)  and  then  uses  the  results  in  (8)~(12),  and  (28a)-(28c) 
to  correct  the  state  estimate.  Each  beam  provides  a  separate  measurement  correction. 


3.6.3  Long  Baseline  Update 

The  acoustic  long  baseline  (LBL)  system  precisely  measures  the  time  of  flight  of  sound  waves 
propagating  through  water.  At  time  to,  the  vehicle  transceiver  generates  a  common  interrogate 
ping.  Each  listening  transponder  hears  this  ping,  each  at  a  different  time,  then  waits  a  specified 
turn-around  time  and  responds.  Eor  example,  transponder  1  responds  250  ms  after  hearing  the  ping, 
transponder  2  responds  at  500  ms,  followed  by  transponder  3  at  750  ms,  and  finally  transponder 
4  at  1000  ms  after  hearing  the  ping.  No  two  transponders  respond  at  the  same  time,  allowing  the 
the  turn-around  time  to  identify  the  transponder.  The  vehicle  receives  the  response  from  the  ith 
transponder  at  time  L.  Eigure  2  illustrates  the  interrogation  cycle. 
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Pito) 


P(U) 


Figure  2:  Long  baseline  interrogation  cycle.  Dotted  lines  indicate  the  acoustic  measurement.  The  solid  line 
indicates  an  arbitrary  vehicle  trajectory. 


The  total  round-trip  time  for  the  ith  transponder  measurement  is  the  travel  time  to  the 
transponder  plus  the  return  travel  time  plus  the  turn-around  time  plus  noise,  or 


Vt 


1 

c(to) 


Pi  -  P{to) 


1 

c{ti) 


p^  -  PiU  ) 


+  Ti  +  rit, 


where  c{t)  is  the  speed  of  sound  in  seawater  and  position  vectors  Pi  and  P{t)  represent  the  position 
of  transponder  i  and  the  vehicle  transceiver  at  time  t,  respectively.  The  vehicle  transceiver  position 
in  tangent  frame  is  P  =  rji  +  Rp(^,  where  i  is  the  sensor  offset  from  the  platform  origin.  Constant 
Ti  is  the  transponder  turn-around  time.  Sensor  noise  nt  is  distributed  according  to  N(0,a^).  The 
measurement  estimate  is  therefore 


yt  = 


c(to) 


Pi  -  Pito) 


-F 


c{ti 


Pi  -  P{ti) 


+  Tj, 


where  the  transponder  position  Pi  and  turn-around  time  Tj  are  known.  The  algorithm  stores  the 
current  state  estimate  at  time  to,  then  recalls  it  to  compute  measurement  residual 


Syt  =  yt-yt 


(35) 


at  time  ti.  We  address  the  nonlinear  2-norm  expression  d{t)  =  -^WPi  ~  -P(^)||  by  representing  it 
using  a  first-order  Taylor  series  approximation  about  state  estimate  x, 


d 


d  + 


dd 

dx^ 


d  + 


dd 

dx^ 


5x. 

X 


x)  + 


1  d^d 
2!  {dx‘^)^ 


x)‘^  +  ... 


The  dependence  on  state  estimates  at  two  separate  times  complicates  the  formulation  of  the  mea¬ 
surement  residual.  It  is  necessary  to  relate  the  error  state  backwards  in  time  to  the  common 
interrogate  ping  via  state  transition  matrix  <h(toTi)-  The  measurement  residual  is  then 


yt 


+  d{ti)  +  Ti  rit^  —  |d(to)  +  di^i)  + 


d{to)  -  d{to) 
dd{to) 


dx~^ 


x{to) 


+ 

5x{to)  + 


d{ti)  -  d{ti 
dd{t., 


dx^ 


x(ti) 


+  nt 

6x{ti)  -F  nt 


D{ti))^{tQ,ti)  +  D{ti) 


6x{ti)  +  nt, 
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where  the  sensor  output  matrix  is 


H  =  D{to)^{to,ti)  +  D{ti). 


(36) 


State  transition  matrix  ^(tQ,ti)  propagates  the  error  state  backwards  in  time  from  time  ti  to  to- 
At  each  time  step,  from  time  to  to  ti,  the  time  propagation  routine  accumulates  transition  matrix 
^(tijto)  according  to  d>(t  +  At, to)  =  ^(t  +  At,  t)<h(t,  to),  where  d>(t  +  At,  t)  is  given  by  equation 
(24).  When  a  measurement  arrives,  the  measurement  correction  routine  computes  the  inverse 
relationship,  where  4>(to,ti)  =  <l>“^(ti, to),  and  applies  the  measurement  correction.  The  nonzero 
partial  derivative  terms  of  matrix  D{t)  are 


dd{t) 

dr]J 

dd{t) 

dp~^ 

dd{t) 


1  (P.-P(t))" 
c(t)  ||P.-P(t)|| 
1  {P^-P{t))'^ 


m  i|p,-p(t)ii 

1 


[Rl{t)£x] 


Pi  -  P{t) 


The  measurement  noise  matrix  is 

R  =  E  (ntnj^  =  at,  (37) 

which  is  a  positive  scalar  for  all  time.  When  the  vehicle  transceiver  emits  a  common  interrogate  ping, 
the  correction  routine  stores  the  current  state  estimate,  while  the  time  propagation  routine  begins 
accumulating  the  state  transition  matrix.  During  this  time,  the  filter  does  not  correct  the  navigation 
state  according  to  equation  (12)  until  the  last  transponder  measurement  arrives.  This  practice 
is  necessary  such  that  we  can  relate  5x{ti)  to  (5x(fo)  via  <h(foTi)-  intermediate  corrections, 
including  those  from  other  aiding  sensors,  propagate  in  5x~ .  When  a  measurement  arrives,  the 
Kalman  filter  evaluates  equations  (35)--(37)  and  then  uses  the  results  in  equations  (8)-(ll)  to 
correct  the  error  state.  When  the  last  transponder  measurement  arrives,  the  filter  corrects  the 
navigation  state  according  to  equation  (12)  and  equations  (28a)~(28c).  Timeout  logic  is  necessary 
to  handle  the  situation  where  the  last  measurement  does  not  arrive.  After  all  transponders  reply 
or  timeout,  the  cycle  repeats.  To  ensure  stability  during  the  initialization  process,  we  initialize 
diagonal  elements  of  PqIo  relating  to  5r]i  and  6c  artificially  small. 


3.6.4  Pressure  Update 

Over  the  vehicle’s  operating  depths,  the  Saunders  and  Fofonoff  (1976)  relationship  [15]  between 
pressure  and  depth  is  nearly  linear,  thus  we  model  the  pressure  sensor  as 

Vz  =  s{r]i  +  Rpi)  +  bz  +  Uz, 

where  s  and  bz  scale  and  offset  the  pressure  measurement,  respectively,  and  s  =  [0, 0,^^].  Vector  £ 
is  the  sensor  position  in  platform  frame.  Sensor  noise  is  distributed  according  to  A"(0,  ci^).  The 
measurement  prediction  is  then 

Vz  =  s(??i  +  R-lP  +  bz, 

where  we  assume  constants  Sz  and  bz  are  known.  Therefore,  the  residual  can  be  calculated  as 

6yz  =  yz-yz  (38) 

=  {s(??i  +  R*p£)  +  bz  +  Uz}  —  {5(571  +  R!pl)  +  bz} 

=  sSyi  —  s[R!p£x]5p  +  Uz- 
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The  sensor  output  matrix  is  then 

H  =  \  s  -s[Rl£x]  0  0  0  0  1  (39) 

with  a  measurement  noise  matrix  of 

R  =  E  =  al,  (40) 

which  is  a  positive  scalar  for  all  time.  When  a  pressure  measurement  arrives,  the  Kalman  filter 
evaluates  equations  (38)-(40)  and  then  uses  the  results  in  equations  (8)-(12),  and  equations  (28a)- 
(28c)  to  correct  the  state  estimate. 

4.  ANALYSIS 

To  analyze  our  filter  implementation,  we  examine  the  navigation  state  error,  covariance,  and  mea¬ 
surement  residuals.  Here,  we  consider  several  scenarios  in  simulation  and  study  the  navigation  state 
error  and  covariance.  In  Section  5,  we  evaluate  the  measurement  residuals  for  experimental  data. 
The  navigation  state  error  x,  which  is  only  available  when  the  true  navigation  state  is  available, 
substantiates  the  filter’s  performance.  The  objective  is  to  drive  the  navigation  state  error  to  zero. 
The  covariance  matrix  provides  a  performance  estimate  of  dx,  where  the  square  root  of  the  diag¬ 
onal  describes  the  error  state  standard  deviation.  We  expect  the  navigation  state  error  to  remain 
within  three  standard  deviations  of  zero.  The  measurement  residuals  describe  the  performance  of 
the  filter’s  measurement  predictions.  These  residuals  should  be  white  noise  when  the  system  and 
measurement  models  approximate  the  true  system. 

The  vehicle  simulation  is  comprehensive.  It  models  a  three-dimensional  environment,  sensor 
performance,  vehicle  dynamics,  and  executes  the  actual  vehicle  software  to  approximate  real-world 
performance.  The  sensor  models  are  similar  to  those  presented  above,  where  in  addition  to  mea¬ 
surement  noise,  we  incorporate  sporadic  sensor  dropouts  and  those  due  to  poor  geometry  and  loss 
of  line  of  sight.  Acoustic  sensor  models  are  simple.  We  do  not  attempt  to  model  acoustic  sound 
propagation  or  multi-path  effects,  and  we  assume  that  acoustic  transmissions  are  instantaneous 
with  respect  to  the  simulation  step  size.  The  vehicle  model  accounts  for  vehicle  dynamics,  hydro¬ 
dynamics,  currents,  and  thruster  forces  based  on  experimental  data.  For  analysis  purposes,  we 
assume  this  model  represents  the  truth  model. 

It  is  helpful  to  understand  the  observability  of  the  system  for  analysis.  Observability  analysis 
allows  one  to  determine  if  it  is  possible  to  estimate  the  error  state  5x  from  the  output  y.  Given 
system  matrix  F  and  a  measurement  output  matrix  H,  we  can  compute  observability  matrix 
O  to  determine  the  states  made  observable  via  the  measurement  correction  associated  with  H. 
Unobservable  states  may  remain  constant  or  diverge,  depending  on  the  model  and  time  propagation 
routine.  The  rate  of  divergence  deserves  future  discussion.  To  check  observability,  construct  the 
observability  matrix 

H 
HE 

.  ? 

HF^-^ 

where  matrix  El  is  a  measurement  output  matrix  or  combination  of  multiple  measurement  output 
matrices  H  =  [HJ ,  ElJ ,  ■  ■  -  ■  The  rank  of  O  indicates  the  number  of  error  states  observable;  if 
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O  is  full  rank,  we  can  estimate  the  entire  error  state  from  output  y  [7,  8].  The  dimension  of  our 
system  is  16,  thus  a  rank  of  16  is  necessary  to  estimate  6x.  The  rank  of  O  is  complementary  to 
the  dimension  of  the  unobservable  subspace.  Clearly,  O  depends  on  x  for  nonlinear  systems.  We 
assume  nominal  conditions  (x  =  [0,0, 0,0, 0, 1500]^)  unless  stated. 

The  scenarios  of  interest  are  where  different  combinations  of  acoustic  sensors  drop  out.  These 
sensors,  which  include  the  LBL  and  DVL,  are  highly  susceptible  to  interference,  so  it  is  important 
to  examine  the  effect  when  their  respective  measurements  are  unavailable.  For  each  scenario,  the 
vehicle  submerges  to  5  m  in  depth,  and  then  executes  a  lawnmower  search  pattern.  The  leg  length 
is  40  m,  with  a  row  spacing  of  5  m.  After  nine  consecutive  rows,  the  vehicle  returns  to  the  beginning 
of  the  first  row  and  repeats  the  mission.  The  vehicle  speed  is  0.5  m/s.  The  acoustic  baseline  outlines 
a  50  X  50  m^  box  around  the  operating  area.  Figure  3  illustrates  the  mission  trajectory.  The  true 
initial  conditions  are  normally  distributed  about  zero  with  realistic  variances,  except  for  the  vehicle 
yaw  angle  and  the  speed  of  sound.  Yaw  is  uniformly  distributed  about  a  circle,  and  speed  of  sound 
is  normally  distributed  about  1450  m/s  with  variance  (15  m/s)^.  All  initial  estimates  are  zero  or 
the  first  available  measurement,  except  for  the  speed  of  sound,  which  is  1500  m/s.  For  the  nominal 
case,  the  system  has  full  observability,  thus  we  expect  to  estimate  5x  and  therefore  x  when  all 
sensors  are  functioning. 


East  (m) 


Figure  3:  North-East  plot  of  one  simulated  mission  from  the  incremental  LBL  dropout  scenario,  where  the 
solid  line  represents  the  navigation  estimate  and  the  dashed  line  represents  the  true  trajectory.  The  four 
circles  represent  the  LBL  transponders.  For  comparison,  the  small  dots  represent  three  and  four-range 
trilateralization  solutions. 

All  results  represent  the  average  of  100  Monte  Carlo  simulations.  The  error  plots  illustrate  the 
true  navigation  state  error  x  and  the  corresponding  standard  deviation  estimate  of  6x  (divided  by 
10).  All  6x  divergence  rates  are  for  one  standard  deviation.  Our  observability  analysis  assumes 
tUj/e  ~  0  to  eliminate  the  attitude  observability  gained  from  the  rotation  of  the  Earth. 
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4.1  INCREMENTAL  LBL  DROPOUTS,  LIMITED  COMPASS  AIDING 


First,  consider  the  ship-hull  inspection  scenario.  Operators  place  one  LBL  transponder  at  each 
corner  of  a  ship  and  deploy  an  inspection  vehicle.  The  vehicle  executes  several  passes  around  and 
underneath  the  hull,  searching  for  objects  of  interest.  In  this  scenario,  the  keel  frequently  obstructs 
line  of  sight  between  the  vehicle  and  one  or  more  transponders,  and,  due  to  magnetic  anomalies, 
vehicles  typically  operate  without  a  magnetic  compass.  To  illustrate  the  system  performance  in 
this  scenario,  we  sequentially  drop  out  LBL  transponders.  Transponder  one  fails  at  600  seconds, 
followed  by  transponders  two,  three,  and  four  at  1200,  1800,  and  2400  seconds,  respectively.  Yaw 
aiding  is  only  available  during  the  first  30  seconds  of  the  mission,  which  allows  the  algorithm  to 
estimate  r]2  such  that  5p  becomes  small  before  disabling  the  compass.  The  DVL,  pressure,  and 
inclinometers  continue  to  aid  the  system.  Figures  4,  5,  and  6  illustrate  the  performance  of  the  east 
position,  azimuth,  and  speed  of  sound  error  states,  respectively. 

On  interval  t  G  [30,  600)  seconds,  where  four  transponders  are  responding  and  the  compass 
is  disabled,  the  nominal  system  has  rank(O)  =  15.  The  unobservable  subspace  spans  linear 
combinations  of  {6r]l{x,y),5p{^lJ)}.  This  result  is  intuitive.  Clearly,  we  cannot  observe  azimuth 
when  the  vehicle  is  stationary.  Observability  analysis  indicates  that  velocity  in  the  horizontal 
direction  promotes  O  to  full  rank;  vertical  velocity  does  not.  Vehicle  rotation,  where  2^2 (r)  7^  0, 
also  promotes  O  to  full  rank. 

The  loss  of  one  transponder  does  not  affect  the  observability  of  the  nominal  system.  The 
unobservable  subspace  remains  unchanged.  Velocity  in  the  horizontal  direction  promotes  O  to  full 
rank.  Rotating,  however,  does  not  promote  O  to  full  rank.  The  unobservable  subspace  transforms 
where  linear  combinations  of  {6r]i{x,y),5p{^),5c}  are  not  observable.  Eliminating  6c  from  the 
error  state  vector  allows  this  maneuver  to  achieve  full  observability. 

Losing  two  transponders  increases  the  dimension  of  'Su  to  2,  where  spans  linear  combinations 
of  {6rii{x,y),6p{il!),6c}.  Velocity  in  the  horizontal  direction  no  longer  achieves  full  observability. 
The  algorithm  cannot  differentiate  between  certain  linear  combinations  of  5r]i{x,y)  and  5c.  To  im- 
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Figure  4:  East  position  error  771(7/)  for  incrementai  LBL  transponder  dropouts  and  iimited  compass  aiding. 
LBL  transponders  drop  out  at  muitipies  of  600  seconds.  The  osciiiations  in  standard  deviation  correiate  to 
the  vehicie  trajectory  and  its  reiationship  to  each  transponder. 
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Figure  5:  Azimuth  error  p{ip)  for  incrementai  LBL  transponder  dropouts  and  iimited  compass  aiding.  LBL 
transponders  drop  out  at  muitipies  of  600  seconds.  The  true  azimuth  error  is  computed  using  equation 

(19a). 


Figure  6:  Sound  speed  error  cfor  incrementai  LBL  transponder  dropouts  and  iimited  compass  aiding.  LBL 
transponders  drop  out  at  muitipies  of  600  seconds. 

prove  results,  one  could  invest  in  aiding  sensors  for  5c  or  assume  the  speed  of  sound  is  deterministic. 
Elimination  of  5c  from  the  error  state  vector  makes  O  full  rank  for  nonzero  horizontal  velocities.  For 
small  operating  regions,  assuming  a  constant  sound  speed  may  produce  acceptable  results.  Figure 
6  shows  that  the  covariance  of  5c  converges  prior  to  the  loss  of  the  second  transponder.  Since  the 
driving  noise  is  small,  the  Kalman  gain  K  is  small  and  the  estimate  remains  steady  over  interval 
t  G  [1200, 1800)  seconds. 

The  loss  of  the  third  transponder  results  in  divergence.  Figures  4  and  5  indicate  the  divergence 
rates  for  east  position  error  and  azimuth  error,  respectively.  On  interval  t  G  [1800,  2400)  seconds, 
where  only  one  transponder  operates,  rank(O)  =  13  for  zero  velocity  and  spans  linear  combina¬ 
tions  of  {5rii{x,y),5p{'ilj),5c}.  For  nonzero  horizontal  velocities,  rank(O)  =  14  and  'Su  spans  linear 
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combinations  of  {5rii{x^y),5c}.  Losing  all  transponders  reduces  observability  to  11  states  and 
spans  linear  combinations  of  {5rii{x,y),5p{'4))},  {(56g(V’)},  and  {(5c}. 

Note  that  as  \\Pi  —  P{t)\\  ^  0  in  equation  (36),  6c  becomes  weakly  observable.  Thus,  for  small 
area  searches  such  as  the  scenario  presented  here,  the  algorithm  may  be  unable  to  estimate  5c 
accurately  as  a  result  of  the  LBL  sensor  performance  characteristics.  We  assume  at  =  1.0  ms  and 
(Tc  =  0.1  m/s.  Figure  6  shows  that  we  cannot  estimate  the  true  speed  of  sound  within  5  m/s  for 
the  given  scenario. 


4.2  INCREMENTAL  DVL  BEAM  DROPOUTS,  NO  LBL  AIDING 

Consider  the  scenario  when  the  LBL  sensor  is  unavailable  and  the  DVL  begins  to  malfunction. 
Beam  one  fails  at  200  seconds,  followed  by  beams  two,  three,  and  four  at  400,  600,  and  800  seconds, 
respectively.  The  attitude  and  pressure  sensors  continue  to  aid  the  system.  Figures  7  and  8  depict 
the  performance  of  velocity  error  vi{u)  and  accelerometer  bias  error  ba{u).  Initially,  the  north 
and  east  error  states,  as  well  as  the  speed  of  sound,  are  not  observable,  and  rank(O)  =  13.  The 
unobservable  subspace  spans  linear  combinations  of  {6r]i{x,y)}  and  {(5c}.  This  result  is  expected 
since  no  sensors  aid  position  or  speed  of  sound.  When  beam  one  fails,  there  is  no  additional  loss 
in  observability.  Subspace  transforms,  but  the  general  relationships  remain  the  same.  Figure  7 
shows  only  a  slight  decrease  in  performance. 


Figure  7:  Velocity  error  z>i(u)  for  incremental  DVL  beam  dropouts  and  no  LBL  aiding.  DVL  beams  drop  out 
at  multiples  of  200  seconds. 
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Figure  8:  Accelerometer  bias  error  ba{u)  for  incremental  DVL  beam  dropouts  and  no  LBL  aiding.  DVL 
beams  drop  out  at  multiples  of  200  seconds. 

The  performance  resulting  for  a  loss  of  a  second  beam  depends  on  the  particular  beam  lost.  If 
the  remaining  beams  lie  within  perpendicular  planes,  the  unobservable  subspace  remains  the  same 
dimension  and  the  performance  loss  is  subtle.  If  the  remaining  beams  lie  within  the  same  plane,  the 
effect  is  detrimental.  Our  simulation  reveals  this  condition,  where  the  dimension  of  increases 
to  5  and  the  rank  of  O  drops  to  11.  Linear  combinations  of  {5r]i{x,y),5iyi,Sba}  and  {dc}  are  not 
observable.  From  Figure  7,  we  see  that  sometimes  the  system  excites  observable  modes  and  the 
error  state  and  covariance  briefly  converge.  To  understand  this  behavior,  we  perform  a  separate 
observability  analysis  for  different  conditions. 

For  nonzero  velocity,  such  as  when  the  vehicle  is  tracking  the  segment  between  two  waypoints, 
has  dimension  5.  For  nonzero  angular  rates,  such  as  when  the  vehicle  achieves  a  waypoint  and  ma¬ 
neuvers  towards  the  next  waypoint,  has  dimension  4.  Linear  combinations  of  {5r]i  {x,y),  5vi ,  5ba} 
and  {(5c}  are  not  observable  for  both  cases.  Finally,  when  the  vehicle  has  a  nonzero  roll  or  pitch 
angle,  has  dimension  3.  For  this  condition,  the  unobservable  subspace  is  similar  to  a  single 
beam  failure.  The  oscillations  in  Figure  7  correspond  to  the  trajectory  of  the  vehicle.  At  each 
waypoint,  the  vehicle  maneuvers  (with  nonzero  angular  rates,  and  a  slight  roll  angle)  towards  the 
next  waypoint  and  the  solution  converges.  When  tracking  the  segment  between  two  waypoints,  the 
solution  diverges.  Losing  a  third  beam  is  similar  to  the  previous  case,  where  the  dimension  of  is 
5.  The  performance  loss  is  subtle  over  interval  t  G  [600,  800).  The  divergence  rates  are  comparable 
to  a  two-beam  failure. 

A  total  loss  of  the  DVL  causes  divergence  rates  to  increase.  The  divergence  rate  corresponding 
to  velocity  error  6i'i{u)  is  5.5  mm/s^  and  to  accelerometer  bias  error  5ba{u)  is  2.4  x  10“^  mm/s^. 
The  rank  of  O  drops  to  9  states,  and  the  system  can  no  longer  maintain  an  acceptable  level  of 
performance.  Note  that  this  scenario  illustrates  the  worst  case.  Intermittent  beam  dropouts  result 
in  only  a  slight  decrease  in  performance  due  to  an  effective  lower  update  rate. 
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5.  EXPERIMENTAL  RESULTS 


The  following  experimental  results  are  from  a  demonstration  at  the  Autonomous  Underwater  Vehi¬ 
cle  Festival  (AUVFest)  in  2007.  The  mission  plan  was  to  submerge  to  3  m  in  depth  for  2  minutes, 
then  execute  two  sets  of  three-dimensional  waypoints  at  1  knot.  The  first  series  of  waypoints  con¬ 
sisted  of  vertically  stacked  legs  between  two  waypoints.  During  this  phase,  the  navigational  goal 
was  to  observe  the  unknown  parameters  (yaw  and  biases)  before  proceeding  to  the  second  series 
of  waypoints  underneath  a  barge.  The  second  series  of  waypoints  consisted  of  a  lawnmower  search 
pattern  in  an  continuous  loop,  similar  to  the  scenario  presented  in  Section  4.  Due  to  severe  mag¬ 
netic  interference  from  the  barge,  we  chose  to  operate  the  vehicle  without  aiding  the  yaw  angle 
with  the  magnetic  compass.  The  acoustic  baseline  outlines  a  36  x  9  m^  box  around  the  second 
series  of  waypoints.  Since  the  true  state  is  not  available,  we  present  analysis  of  select  error  state 
covariances  and  measurement  residuals.  Note  that  the  sensor  data  presented  here  is  identical  to 
that  presented  in  reference  [4];  however,  here  we  reprocessed  the  raw  data  through  the  algorithms 
presented  in  this  report. 

Position  accuracy  is  a  critical  metric  for  inspection-class  vehicles.  This  information  allows 
operators  to  localize  objects  of  interest,  reacquire  contacts,  and  navigation  through  complex  envi¬ 
ronments.  Figure  9  illustrates  the  estimated  standard  deviation  of  the  north,  east,  and  down  error 
states  during  the  AUVFest  demonstration.  These  results  are  consistent  with  the  simulation  results 
in  Figure  4.  The  convergence  is  dependent  on  the  acoustic  baseline  geometry,  vehicle  trajectory, 
and  several  important  factors.  These  factors  include  accuracy  of  the  baseline  calibration,  estimate 
of  the  speed  of  sound,  and  the  estimate  of  the  vehicle  attitude. 


Time  (s) 

Figure  9:  North,  east,  and  down  position  error  6rii  standard  deviation.  The  standard  deviation  estimate 
converges  to  13  cm,  17  cm,  and  0.7  cm  for  north,  east,  and  down  position  errors,  respectiveiy.  These  resuits 
are  subject  to  the  baseiine  configuration  and  do  not  necessariiy  mean  fji  is  this  accurate. 

We  determine  the  acoustic  baseline  geometry  prior  to  deployment  via  acoustic  calibration.  Using 
a  spare  transponder,  the  calibration  algorithm  measures  round-trip  travel  times  to  each  transponder 
and  among  all  transponders.  The  algorithm  assumes  the  relative  organization  of  transponders  to 
formulate  a  geometric  solution,  which  provides  estimates  for  the  transponder  locations  and  the 
speed  of  sound.  For  our  demonstration,  it  estimated  a  sound  speed  of  1491  m/s.  The  transponder 


21 


locations  are  in  a  local  coordinate  system,  where  transponder  1  identifies  the  origin  and  the  vector 
from  transponder  1  to  transponder  3  defines  the  y-axis.  We  transform  these  coordinates  into  tangent 
plane  coordinates  for  navigation.  The  current  hardware  implementation  requires  us  to  perform  this 
procedure  prior  to  operating  the  vehicle.  It  is  not  possible  to  estimate  the  baseline  online  with  our 
current  hardware.  Clearly,  a  poor  baseline  calibration  will  degrade  performance. 

Analyzing  the  LBL  round-trip  measurement  residuals  provides  insight  into  the  baseline  cali¬ 
bration.  Figure  10  represents  the  measurement  residuals  for  transponder  1.  Several  features  are 
apparent.  First,  the  residuals  do  not  resemble  white  noise.  Second,  approximately  7  percent  of 
the  data  lies  beyond  three  standard  deviations  of  its  expected  value.  We  attribute  these  erroneous 
measurements  to  acoustic  noise  and  multi-path  effects.  Simple  hltering  techniques  tend  to  produce 
inconsistent  results  due  to  the  slow  update  rate  and  position  uncertainty.  Our  current  algorithm 
assumes  all  measurements  are  valid.  Another  notable  feature  evident  in  the  data  is  a  profound 
oscillation.  This  oscillation  is  evident  in  all  transponder  residuals  and  is  consistent  with  a  poor 
baseline  calibration  in  simulation.  Simulation  results  confirm  that  misalignment  of  one  transponder 
will  hinder  performance  of  the  entire  system.  The  oscillation  correlates  to  the  vehicle  trajectory. 
Figure  9  also  shows  an  oscillatory  pattern  in  the  north  and  east  error  state  standard  deviations. 
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Figure  10:  Long  baseline  transponder  1  residual.  This  residual  exhibits  an  oscillatory  pattern  that  is  consis¬ 
tent  with  baseline  misalignment  and  the  vehicle  trajectory.  All  transponder  residuals  exhibit  similar  patterns. 
The  horizontal  lines  indicate  one,  two,  and  three  standard  deviations  about  zero,  where  a  =  \/HP  H"'  -i-  R. 

Inaccuracies  in  the  speed  of  sound  estimate  can  also  cause  oscillations  in  the  round-trip  mea¬ 
surement  residuals.  Figure  11  illustrates  the  estimate  of  the  speed  of  sound.  The  calibration  routine 
estimated  1491  m/s.  The  navigation  algorithm,  however,  converged  near  1420  m/s,  which  is  unreal¬ 
istic,  given  environmental  conditions.  Possible  explanations  include  unknown  biases,  scale  factors, 
and  inaccuracies  in  the  sensor  clock  frequency. 

The  azimuth  error  is  of  particular  interest  since  yaw  is  an  essential  control  signal  and  is 

only  observable  via  the  long  baseline  system.  Poor  estimation  of  error  state  5p{'tp),  and  5p,  generally, 
will  result  in  inadequate  navigation  and  control  performance.  Figure  12  illustrates  the  azimuth  and 
corresponding  gyro  bias  error  standard  deviations.  The  azimuth  error  Sp{'il>)  standard  deviation 
converges  to  0.5  deg.  This  result  does  not  necessarily  mean  p{'ilj)  is  this  accurate,  however,  it  is 
consistent  with  the  simulation  results  in  Figure  5.  Note  that  the  performance  specifications  for  our 
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Figure  1 1 :  Sound  speed  estimate  c.  Thin  iines  indicate  three  standard  deviations  beyond  the  estimate.  The 
estimate  converges  to  approximateiy  1420  m/s,  which  is  suspicious,  given  the  environmentai  conditions. 

fiber-optic  gyro  include  a  bias  stability  of  <20  deg/lir  and  random  walk  of  <0.4  deg/hra . 

Estimation  of  the  unknown  accelerometer  and  gyro  bias  parameters  is  a  key  aspect  of  our 
approach.  Poor  estimation  of  these  parameters  leads  to  inaccuracies  in  the  time  propagation  of  the 
navigation  state  vector,  and  thus  poor  measurement  predictions  for  the  aiding  sensors.  Figure  13 
illustrates  the  convergence  of  the  unknown  bias  parameters.  All  parameters  converged  to  reasonable 
values  within  the  first  500  seconds.  The  convergence  rate  is  subject  to  the  aiding  sensors. 

Figure  14  illustrates  the  measurement  residuals  for  DVL  beam  1.  Initially,  the  residuals  are 
noisy  as  the  estimates  for  platform  velocity,  attitude,  and  biases  estimates  are  converging.  After 
the  filter  reaches  steady  state,  the  residuals  resemble  white  noise.  Residuals  beyond  three  standard 
deviations  are  ignored. 


6.  FUTURE  WORK 

The  experimental  results  indicate  that  inaccuracies  in  the  acoustic  baseline  will  degrade  perfor¬ 
mance.  The  algorithm  presented  in  this  article  does  not  attempt  to  estimate  the  individual 
transponder  locations,  nor  does  it  account  for  uncertainty  in  the  baseline.  To  improve  results, 
it  may  be  necessary  to  estimate  the  baseline  as  well  as  unknown  parameters,  such  as  the  clock 
scale  factors  and  biases.  Recent  advancements  in  acoustic  baseline  sensor  capabilities  permit  more 
sophisticated  algorithms.  These  capabilities  include  baseline  measurements  via  acoustic  relaying, 
Doppler  shift  measurements,  and  bearing  measurements.  It  is  also  possible  to  communicate  infor¬ 
mation  among  acoustic  nodes. 

Baseline  measurements  via  acoustic  relaying  provide  travel  time  measurements  between  transpon¬ 
ders.  This  information  permits  integrating  calibration  routines  into  the  navigation  algorithms.  Con¬ 
sider  the  following  measurement  cycle,  where  the  objective  is  to  obtain  an  estimate  of  the  travel  time 
between  transponders  A  and  B.  To  initiate  the  cycle,  the  vehicle  V  interrogates  A  for  the  travel 
time  between  A  and  B.  Transponder  A  receives  this  request  from  V,  waits  a  known  turn-around 
time,  then  interrogates  B.  Transponder  B  receives  this  request  from  A,  waits  a  known  turn-around 
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Figure  12:  Semi-log  plots  of  azimuth  and  gyro  bias  error  standard  deviations.  Azimuth  error  5p{\p)  standard 
deviation  converges  to  0.5  deg.  Gyro  bias  error  standard  deviation  converges  to  1.5  x  10“^  deg/s. 

time,  then  replies.  Finally,  transponder  A  receives  the  reply  from  B,  and  then  transmits  the  travel 
time  measurement  to  the  vehicle  after  a  known  turn-around  time.  Since  the  vehicle  can  hear  the 
communications  between  A  and  B,  four  pieces  of  information  can  be  extracted  from  this  measure¬ 
ment  cycle.  The  vehicle  obtains  travel  times  for  V^A^V ,  V^A^B^V ,  V^A^B^A^V ,  and 
the  direct  measurement  A^B^A.  This  capability  allows  navigation  algorithms  to  make  intelligent 
LBL  measurement  cycle  decisions  to  optimize  overall  system  performance.  Further  performance 
can  be  obtained  by  manipulating  the  turn-around  times  as  a  function  of  vehicle  position  in  order 
to  maximum  the  update  rate. 

Doppler  shift  measurements  allow  the  vehicle  to  measure  the  relative  velocity  between  the 
vehicle  and  the  transponder.  Unique  encoding  in  the  acoustic  communications  makes  this  capability 
possible.  The  corresponding  measurement  correction  is  similar  to  the  correction  presented  in  Section 
3.6.2,  however,  in  this  case  the  measurement  direction  is  not  deterministic. 

Bearing  measurements  are  possible  using  an  ultrashort  baseline  (USBL)  configuration  fixed 
to  the  vehicle.  The  USBL  is  a  method  of  underwater  acoustic  positioning  where  the  transceiver 
incorporates  multiple  hydrophones,  all  within  a  half-wavelength  of  each  other.  By  computing  the 
relative  phase  between  hydrophones,  the  bearing  of  the  acoustic  source  can  be  resolved.  Bearing 
measurements  are  particularly  attractive,  as  they  could  provide  frequent  measurement  corrections 
while  receiving  time-consuming  acoustic  communications  from  topside. 

Clearly,  additional  sensors  and/or  higher  quality  sensors  will  improve  performance.  In  this  re¬ 
port,  we  formulated  a  robust  navigation  algorithm  for  autonomous  underwater  vehicles  using  an 
error  state  formulation  of  the  Kalman  filter.  The  algorithm  incorporates  unique  acoustic  sensors, 
such  as  the  Doppler  velocity  log  and  a  long  baseline  system,  and  can  estimate  critical  bias  param¬ 
eters  to  improve  performance.  Future  work  will  address  inaccuracies  in  the  acoustic  baseline  to 
improve  results  further. 
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Figure  13:  Accelerometer  and  gyro  bias  estimates.  Biases  converge  to  reasonable  values  quickly. 
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Figure  14:  Doppler  velocity  log  beam  1  residual.  The  dashed  lines  indicate  three  standard  deviations,  where 
u  =  +  R.  Residuals  beyond  three  standard  deviations  are  ignored  as  indicated  by  the  x  marker. 
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